# vim: expandtab:ts=4:sw=4
import numpy as np
import scipy.linalg


"""
Table for the 0.95 quantile of the chi-square distribution with N degrees of
freedom (contains values for N=1, ..., 9). Taken from MATLAB/Octave's chi2inv
function and used as Mahalanobis gating threshold.
具有 N 个自由度的卡方分布的 0.95 分位数表（包含 N=1、...、9 的值）。 取自 MATLAB/Octave 的
chi2inv 函数作为马氏门控阈值。
服从自由度为9的卡方分布，
"""
chi2inv95 = {
    1: 3.8415,
    2: 5.9915,
    3: 7.8147,
    4: 9.4877,
    5: 11.070,
    6: 12.592,
    7: 14.067,
    8: 15.507,
    9: 16.919}

class KalmanFilter(object):
    """
    一个标准的卡尔曼滤波器，用来在图像空间中跟踪bounding boxes
    有关目标状态的变量为:x,y,a,h,vx,vy,va,vh
    x,y表示bbox的中心坐标
    a表示bbox宽高比
    h表示bbox宽高
    vx,vy,va,vh表示对应变量的速度，这是一个匀速卡尔曼滤波模型
    物体运动遵循恒定速度模型。 边界框位置 (x, y, a, h) 被视为状态空间的直接观察（线性观察模型）。
    The 8-dimensional state space
        x, y, a, h, vx, vy, va, vh
    Object motion follows a constant velocity model. The bounding box location
    (x, y, a, h) is taken as direct observation of the state space (linear
    observation model).
    """

    def __init__(self):
        ndim, dt = 4, 1.    #

        #创建卡尔曼滤波模型的8x8矩阵，创建一个8x8的状态转移矩阵F
        self._motion_mat = np.eye(2 * ndim, 2 * ndim)
        for i in range(ndim):
            self._motion_mat[i, ndim + i] = dt
        #创建卡尔曼滤波模型的4x8矩阵，H
        self._update_mat = np.eye(ndim, 2 * ndim)

        # 相对于当前状态估计选择运动和观察不确定性。 这些权重控制模型中的不确定性。 这有点骇人听闻。
        self._std_weight_position = 1. / 20
        self._std_weight_velocity = 1. / 160

    def initiate(self, measurement):
        """Create track from unassociated measurement.

        Parameters
        ----------
        measurement : ndarray
            Bounding box coordinates (x, y, a, h) with center position (x, y),
            aspect ratio a, and height h.

        Returns
        -------
        (ndarray, ndarray)
            Returns the mean vector (8 dimensional) and covariance matrix (8x8
            dimensional) of the new track. Unobserved velocities are initialized
            to 0 mean.

        """
        mean_pos = measurement
        mean_vel = np.zeros_like(mean_pos)
        mean = np.r_[mean_pos, mean_vel]

        std = [
            2 * self._std_weight_position * measurement[3],
            2 * self._std_weight_position * measurement[3],
            1e-2,
            2 * self._std_weight_position * measurement[3],
            10 * self._std_weight_velocity * measurement[3],
            10 * self._std_weight_velocity * measurement[3],
            1e-5,
            10 * self._std_weight_velocity * measurement[3]]
        covariance = np.diag(np.square(std))
        return mean, covariance

    def predict(self, mean, covariance):
        """
        运行卡尔曼滤波预测步骤，计算（8x8的矩阵和协方差）

        Parameters
        ----------
        mean : ndarray:(8,)
            上一时刻的track的后验估计均值，一个8维均值向量
        covariance : ndarray:(8x8)
            上一时刻的track的后验估计协方差，8x8的协方差矩阵

        Returns
        -------
        (ndarray, ndarray)
            返回预测状态的均值向量和协方差矩阵。 未观察到的速度被初始化为 0 均值。
        """
        std_pos = [ #位置
            self._std_weight_position * mean[3],
            self._std_weight_position * mean[3],
            1e-2,
            self._std_weight_position * mean[3]]
        std_vel = [ #速度
            self._std_weight_velocity * mean[3],
            self._std_weight_velocity * mean[3],
            1e-5,
            self._std_weight_velocity * mean[3]]
        #初始化噪声矩阵Q
        motion_cov = np.diag(np.square(np.r_[std_pos, std_vel]))
        #均值更新式和协方差更新式,M' = FM
        mean = np.dot(self._motion_mat, mean)
        #C'=FCF^T+Q
        covariance = np.linalg.multi_dot((
            self._motion_mat, covariance, self._motion_mat.T)) + motion_cov

        return mean, covariance

    def project(self, mean, covariance):
        """
        将均值和协方差的状态分布矩阵/向量投影到测量空间
        mean : ndarray 状态均值向量，8维数组
        covariance : ndarray 状态协方差矩阵(8x8维).
        """
        std = [
            self._std_weight_position * mean[3],
            self._std_weight_position * mean[3],
            1e-1,
            self._std_weight_position * mean[3]]
        # 初始化噪声矩阵R,4x4对角矩阵，对角线上值分别为中心点xy和ah的噪声
        innovation_cov = np.diag(np.square(std))
        # 将均值向量映射到detection空间，即Hx'
        mean = np.dot(self._update_mat, mean)
        #将协方差矩阵映射到检测空间，即HP'H^T
        covariance = np.linalg.multi_dot((
            self._update_mat, covariance, self._update_mat.T))
        #返回mean,
        return mean, covariance + innovation_cov

    def update(self, mean, covariance, measurement):
        """Run Kalman filter correction step.

        Parameters
        ----------
        mean : ndarray
            预测的状态均值向量 (8 dimensional).
        covariance : ndarray
            预测的状态协方差矩阵 (8x8 dimensional).
        measurement : ndarray
            4维度的测量向量(x, y, a, h)

        Returns
        -------
        (ndarray, ndarray)
            Returns the measurement-corrected state distribution.
        """
        #将mean和covariance映射到检测空间,Hx'和S
        projected_mean, projected_cov = self.project(mean, covariance)
        #cholesky分解
        chol_factor, lower = scipy.linalg.cho_factor(
            projected_cov, lower=True, check_finite=False)
        #卡尔曼增益计算K
        kalman_gain = scipy.linalg.cho_solve(
            (chol_factor, lower), np.dot(covariance, self._update_mat.T).T,
            check_finite=False).T
        #得到测量值的映射结果y=z-Hx'，z是检测结果
        innovation = measurement - projected_mean
        #x = x'+Ky = x'+K(z-Hx')
        new_mean = mean + np.dot(innovation, kalman_gain.T)
        #P = (I-KH)P'，代码是P=P'-KSK^T
        new_covariance = covariance - np.linalg.multi_dot((
            kalman_gain, projected_cov, kalman_gain.T))
        return new_mean, new_covariance

    def gating_distance(self, mean, covariance, measurements,
                        only_position=False):
        """
        计算measurements和状态distribution之间的gating距离
        可以从 `chi2inv95` 中获得合适的距离阈值。
        如果only_position=False，那么卡方分布自由度=4,否则chi-square分布的自由度=2

        Parameters
        ----------
        mean : ndarray
            状态分布上的8维均值向量.
        covariance : ndarray
            状态分布上的8x8协方差.
        measurements : ndarray
            一个 Nx4 维矩阵，包含 N 个测量值，每个测量值的格式为 (x, y, a, h)，
            其中 (x, y) 是bbox的中心位置，a 是宽高比，h 是高度。
        only_position : Optional[bool]
            如果为 True，则仅针对边界框中心位置进行距离计算

        Returns
        -------
        ndarray
            返回一个长度为 N 的数组，其中第 i 个元素包含
            (mean, covariance) 和 `measurements[i]` 之间的马氏距离平方。
        """
        #将mean和convariance映射到检测空间
        mean, covariance = self.project(mean, covariance)
        if only_position:
            mean, covariance = mean[:2], covariance[:2, :2]
            measurements = measurements[:, :2]
        '''
        这里使用d=x-u，这个代表马氏距离的里面的项，所以
        (maha_dis)^2=(d)^T * (cov)^(-1) * d
        '''
        d = measurements - mean
        '''
        cholesky分解，A=LL^T，它要求被分解的矩阵是一个正定对称矩阵，求解出的L
        的下三角元素也是>0的。cholesky分解可以用来求解AX=B的解问题。
        covariance = cholesky_factor*(cholesky_factor)^T
        这里求解出的cholesky_factor是一个对角矩阵，看成L的话，那么A=L*L
        '''
        cholesky_factor = np.linalg.cholesky(covariance)
        '''
        求解上/下三角矩阵a的方程组ax=b，在这里是求解cholesky_factor*z=d.T,
        且该方程组具有唯一解
        '''
        z = scipy.linalg.solve_triangular(
            a=cholesky_factor,
            b=d.T,
            lower=True,#是否只使用a的下三角元素
            check_finite=False,#是否检测a是否含有无限数的情况，不检查会加快性能
            overwrite_b=True)#是否覆盖b中的数据，覆盖可能会提高性能
        #z*z表示逐元素平方，z表示马哈拉诺比斯距离
        squared_maha = np.sum(z * z, axis= 0)#马氏距离的平方
        return squared_maha
